#include <stddef.h>
#include <peripheral/timer.h>
#include "CoreTimerDelay.h"
#include "String.h"


const unsigned int countPerMicroSec = ((SYS_CLK_FREQ/1000000)/2);
CoreDelay_DelayCallback delaycallback[COREDELAY_MAXCLIENTS];
CoreDelay_BusyCallback busycallback[COREDELAY_MAXCLIENTS];
// Set busy and inprogess true to disallow client access.  This will
// force the client to call init.
static bool busy = true;
static bool inprogress = true;
static bool oneShotFlag;
static uint8_t count;
static uint8_t count_index;
static unsigned int delays[COREDELAY_MAXIDS];

void CoreDelay_Init(){
    // All callbacks are cleared.
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        delaycallback[index] = NULL;
        busycallback[index] = NULL;
    }
    // All delays are cleared.  Do not depend on these being 0 if not used.
    for (index = 0; index < COREDELAY_MAXIDS; index++){
        delays[index] = 0;
    }
    // Set busy and inprogess to allow clients access.
    busy = false;
    inprogress = false;
    count = 0;
    count_index = 0;
}

// Callback that informs clients that a delay has expired.
bool CoreDelay_SetDelayCallback(CoreDelay_DelayCallback function){
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (delaycallback[index] == NULL){
            delaycallback[index] = function;
            return true;
        }
    }
    return false;
}

// Callback that informs clients when the delay timer is busy
bool CoreDelay_SetBusyCallback(CoreDelay_BusyCallback function){
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (busycallback[index] == NULL){
            busycallback[index] = function;
            return true;
        }
    }
    return false;
}

// Remove client from callback
void CoreDelay_RemoveDelayCallback(CoreDelay_DelayCallback function){
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (delaycallback[index] == function){
            delaycallback[index] = NULL;
        }
    }
}

// Remove client from callback
void CoreDelay_RemoveBusyCallback(CoreDelay_BusyCallback function){
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (busycallback[index] == function){
            busycallback[index] = NULL;
        }
    }
}

// Returns true if in use
bool CoreDelay_Busy(){
    return busy;
}

void CoreDelay_SendBusy(bool busy){
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (busycallback[index] != NULL){
            busycallback[index](busy);
        }
    }
}

// Returns true on success or false if busy with other client
// If not busy then this function will block until the delay expires.
bool CoreDelay_Sync_us(unsigned int us){
    if (busy) return 0;

    unsigned int targetCount;
    unsigned int backupCount;
    unsigned char loop = 0;

    // backup current count of the core timer.
    backupCount = ReadCoreTimer();
    // Core timer increments every 2 sys clock cycles.
    // Calculate the counts required to complete "us".
    targetCount = countPerMicroSec * us;
    // Restart core timer.
    WriteCoreTimer(0);
    // Wait till core timer completes the count.
    while(ReadCoreTimer() < targetCount);
    // Restore count back.
    WriteCoreTimer(backupCount + targetCount);
}

void CoreDelay_StartDelay(unsigned int delay){
    // Set up the time for the call back
    mConfigIntCoreTimer((CT_INT_OFF | COREDELAY_IP | COREDELAY_ISP));
    WriteCoreTimer(0);
    _mtc0(11,0,delay);
    mEnableIntCoreTimer();
}

// Returns true on success or false if busy
// Schedules 1 delay for the client
bool CoreDelay_Async_us1(unsigned int us1){
    unsigned int delays[1] = {us1};
    return CoreDelay_Async_usA(false, delays, 1);
}

// Returns true on success or false if busy
// Schedules 2 delays for the client.
// If oneShot then the client must call next to  start the following delay
bool CoreDelay_Async_us2(bool oneShot, unsigned int us1, unsigned int us2){
    unsigned int delays[2] = {us1, us2};
    return CoreDelay_Async_usA(oneShot, delays, 2);
}

// Returns true on success or false if busy
// Schedules 3 delays for the client.
// If oneShot then the client must call next to  start the following delay
bool CoreDelay_Async_us3(bool oneShot, unsigned int us1, unsigned int us2, unsigned int us3){
    unsigned int delays[3] = {us1, us2, us3};
    return CoreDelay_Async_usA(oneShot, delays, 3);
}

// If size > COREDELAY_MAXIDS will return false
// Returns true on success or false if busy
// Schedules multiple delays for the client.
// If oneShot then the client must call next to  start the following delay
bool CoreDelay_Async_usA(bool oneShot, unsigned int* usA, uint8_t size){
    if (busy) return false;
    if (size > COREDELAY_MAXIDS) return false;

    busy = true;
    oneShotFlag = oneShot;
    CoreDelay_SendBusy(busy);
    uint8_t index;
    for (index = 0; index < size; index++){
        delays[index] = *(usA+index);
    }
    count = size;
    count_index = 0;
    inprogress = true;
    CoreDelay_StartDelay(delays[count_index]);
    return true;
}

// Returns true if delay is started.
bool CoreDelay_Next(){
    if (busy && !inprogress){
        count_index++;
        if (count_index < count){
            inprogress == true;
            CoreDelay_StartDelay(delays[count_index]);
            return true;
        }
    }
    return false;
}

// Stops all activity
void CoreDelay_Abort(){
    mDisableIntCoreTimer();
    inprogress = false;
    busy = false;
    count = 0;
    count_index = 0;

    CoreDelay_SendBusy(busy);
}

// Core timer interrupt
void __ISR( _CORE_TIMER_VECTOR, COREDELAY_INT) CoreInterrupt( void){
    mDisableIntCoreTimer();
    uint8_t index;
    for (index = 0; index < COREDELAY_MAXCLIENTS; index++){
        if (delaycallback[index] != NULL){
            delaycallback[index](count_index + 1);
        }
    }
    inprogress = false;
    if (count_index == count - 1){
        busy = false;
        CoreDelay_SendBusy(busy);
        count = 0;
        count_index = 0;
    } else {
        if (oneShotFlag == false){
            CoreDelay_Next();
        }
    }
    mCTClearIntFlag();
}
